#include <iostream>
#include <Eigen/Dense>
#include <cmath>

class GpsJacobianComparator {
public:
    struct State {
        Eigen::Vector3d G_p_I;    // IMU position in global frame
        Eigen::Matrix3d G_R_I;    // Rotation from IMU to global frame
        Eigen::Vector3d velocity; // velocity (not used in GPS observation)
        Eigen::Vector3d bg;       // gyro bias (not used)
        Eigen::Vector3d ba;       // accel bias (not used)
    };

    struct GpsData {
        Eigen::Vector3d lla;      // latitude, longitude, altitude
    };

    GpsJacobianComparator(const Eigen::Vector3d& I_p_Gps) 
        : I_p_Gps_(I_p_Gps) {}

    // Analytical method (original implementation)
    void ComputeJacobianAnalytical(const State& state,
                                 Eigen::Matrix<double, 3, 15>* jacobian,
                                 Eigen::Vector3d* residual,
                                 const Eigen::Vector3d& G_p_Gps) {
        const Eigen::Vector3d& G_p_I = state.G_p_I;
        const Eigen::Matrix3d& G_R_I = state.G_R_I;

        // Compute residual: r = z - h(x)
        *residual = G_p_Gps - (G_p_I + G_R_I * I_p_Gps_);

        // Compute Jacobian: H = ∂h/∂x
        jacobian->setZero();
        jacobian->block<3, 3>(0, 0)  = Eigen::Matrix3d::Identity();  // ∂h/∂p
        jacobian->block<3, 3>(0, 6)  = -G_R_I * SkewSymmetric(I_p_Gps_);  // ∂h/∂θ
    }

    // Numerical differentiation method
    void ComputeJacobianNumerical(const State& state,
                                Eigen::Matrix<double, 3, 15>* jacobian,
                                Eigen::Vector3d* residual,
                                const Eigen::Vector3d& G_p_Gps,
                                double delta = 1e-8) {
        const Eigen::Vector3d& G_p_I = state.G_p_I;
        const Eigen::Matrix3d& G_R_I = state.G_R_I;

        // Compute residual at nominal point
        *residual = G_p_Gps - (G_p_I + G_R_I * I_p_Gps_);

        // Observation function h(x)
        auto observation_func = [&](const State& s) -> Eigen::Vector3d {
            return s.G_p_I + s.G_R_I * I_p_Gps_;
        };

        // Nominal observation
        Eigen::Vector3d h0 = observation_func(state);

        // Perturb each state variable and compute finite differences
        jacobian->setZero();
        State perturbed_state = state;

        // 1. Perturb position (states 0-2)
        for (int i = 0; i < 3; ++i) {
            perturbed_state.G_p_I = state.G_p_I;
            perturbed_state.G_p_I[i] += delta;
            
            Eigen::Vector3d h_perturbed = observation_func(perturbed_state);
            jacobian->col(i) = (h_perturbed - h0) / delta;
        }

        // 2. Perturb attitude (states 6-8) using exponential map
        for (int i = 0; i < 3; ++i) {
            perturbed_state = state;
            
            // Create small rotation vector
            Eigen::Vector3d delta_theta = Eigen::Vector3d::Zero();
            delta_theta[i] = delta;
            
            // Apply perturbation: R_perturbed = R * Exp(δθ)
            Eigen::Matrix3d delta_R = ExponentialMap(delta_theta);
            perturbed_state.G_R_I = state.G_R_I * delta_R;
            
            Eigen::Vector3d h_perturbed = observation_func(perturbed_state);
            jacobian->col(6 + i) = (h_perturbed - h0) / delta;
        }

        // Note: Velocity, biases have zero Jacobian for GPS observation
        // So we don't need to perturb them
    }

    // Convert LLA to ENU (simplified)
    Eigen::Vector3d ConvertLLAToENU(const Eigen::Vector3d& init_lla, 
                                   const Eigen::Vector3d& current_lla) {
        // Simplified conversion - in real implementation, use proper geodesy formulas
        double lat_scale = 111320.0;  // meters per degree latitude
        double lon_scale = 111320.0 * cos(init_lla[0] * M_PI / 180.0);
        
        return Eigen::Vector3d(
            (current_lla[1] - init_lla[1]) * lon_scale,
            (current_lla[0] - init_lla[0]) * lat_scale,
            current_lla[2] - init_lla[2]
        );
    }

    // Compare both methods
    void CompareMethods(const Eigen::Vector3d& init_lla, 
                       const GpsData& gps_data, 
                       const State& state) {
        // Convert GPS measurement to ENU
        Eigen::Vector3d G_p_Gps = ConvertLLAToENU(init_lla, gps_data.lla);

        // Analytical method
        Eigen::Matrix<double, 3, 15> jacobian_analytical;
        Eigen::Vector3d residual_analytical;
        ComputeJacobianAnalytical(state, &jacobian_analytical, &residual_analytical, G_p_Gps);

        // Numerical method
        Eigen::Matrix<double, 3, 15> jacobian_numerical;
        Eigen::Vector3d residual_numerical;
        ComputeJacobianNumerical(state, &jacobian_numerical, &residual_numerical, G_p_Gps);

        // Compare results
        std::cout << "=== GPS Jacobian Comparison ===" << std::endl;
        std::cout << "Residual difference: " << (residual_analytical - residual_numerical).norm() << std::endl;
        
        // Compare position Jacobian (columns 0-2)
        Eigen::Matrix3d Hp_analytical = jacobian_analytical.block<3, 3>(0, 0);
        Eigen::Matrix3d Hp_numerical = jacobian_numerical.block<3, 3>(0, 0);
        std::cout << "Position Jacobian difference: " << (Hp_analytical - Hp_numerical).norm() << std::endl;

        // Compare attitude Jacobian (columns 6-8)
        Eigen::Matrix3d Htheta_analytical = jacobian_analytical.block<3, 3>(0, 6);
        Eigen::Matrix3d Htheta_numerical = jacobian_numerical.block<3, 3>(0, 6);
        std::cout << "Attitude Jacobian difference: " << (Htheta_analytical - Htheta_numerical).norm() << std::endl;

        // Print detailed comparison
        std::cout << "\n=== Detailed Jacobian Comparison ===" << std::endl;
        std::cout << "Analytical Position Jacobian:\n" << Hp_analytical << std::endl;
        std::cout << "Numerical Position Jacobian:\n" << Hp_numerical << std::endl;
        
        std::cout << "Analytical Attitude Jacobian:\n" << Htheta_analytical << std::endl;
        std::cout << "Numerical Attitude Jacobian:\n" << Htheta_numerical << std::endl;

        // Verify with different step sizes
        std::cout << "\n=== Numerical Accuracy Test ===" << std::endl;
        for (double step : {1e-6, 1e-7, 1e-8, 1e-9}) {
            Eigen::Matrix<double, 3, 15> jacobian_test;
            Eigen::Vector3d residual_test;
            ComputeJacobianNumerical(state, &jacobian_test, &residual_test, G_p_Gps, step);
            
            Eigen::Matrix3d Htheta_test = jacobian_test.block<3, 3>(0, 6);
            double error = (Htheta_analytical - Htheta_test).norm();
            std::cout << "Step size " << step << ": error = " << error << std::endl;
        }
    }

private:
    Eigen::Vector3d I_p_Gps_;

    // Skew-symmetric matrix
    Eigen::Matrix3d SkewSymmetric(const Eigen::Vector3d& v) {
        Eigen::Matrix3d M;
        M << 0, -v(2), v(1),
             v(2), 0, -v(0),
             -v(1), v(0), 0;
        return M;
    }

    // Exponential map for rotation vector (first-order approximation)
    Eigen::Matrix3d ExponentialMap(const Eigen::Vector3d& theta) {
        double theta_norm = theta.norm();
        if (theta_norm < 1e-10) {
            return Eigen::Matrix3d::Identity() + SkewSymmetric(theta);
        }
        
        Eigen::Vector3d axis = theta / theta_norm;
        double sin_theta = sin(theta_norm);
        double cos_theta = cos(theta_norm);
        double one_minus_cos = 1 - cos_theta;
        
        Eigen::Matrix3d K = SkewSymmetric(axis);
        return Eigen::Matrix3d::Identity() + sin_theta * K + one_minus_cos * K * K;
    }
};

// Test function

//前向差分法
int main() {
    // GPS installation position in IMU frame
    Eigen::Vector3d I_p_Gps(0.1, 0.2, 0.3);  // 10cm forward, 20cm right, 30cm up
    
    GpsJacobianComparator comparator(I_p_Gps);

    // Test state
    GpsJacobianComparator::State state;
    state.G_p_I = Eigen::Vector3d(10.0, 20.0, 30.0);  // 10m East, 20m North, 30m Up
    
    // 45 degree rotation around z-axis
    double angle = M_PI / 4;  // 45 degrees
    state.G_R_I << cos(angle), -sin(angle), 0,
                   sin(angle), cos(angle), 0,
                   0, 0, 1;

    // GPS data
    Eigen::Vector3d init_lla(40.0, 116.0, 100.0);  // Initial reference
    GpsJacobianComparator::GpsData gps_data;
    gps_data.lla = Eigen::Vector3d(40.0001, 116.0001, 100.5);  // Slightly different

    // Run comparison
    comparator.CompareMethods(init_lla, gps_data, state);

    return 0;
}